#include "VsomeipServer.h"
#include <vsomeip/vsomeip.hpp>
#include <iomanip>
#include <iostream>
#include <sstream>
#include <vector>
#include <set>
#include <cstring>
    
VsomeipServer::VsomeipServer()
 : itsApp(vsomeip::runtime::get()->create_application("PBOXGGASrv"))
 , isRunning(false), notify_thread_(), itsVsomeipThread()
{
}


VsomeipServer& VsomeipServer::instance()
{
    static VsomeipServer sev;
    return sev;
}

void VsomeipServer::startup()
{
    init();
    start();
}

void VsomeipServer::teardown()
{
    stop_offer();
    stop();
}

bool VsomeipServer::init() 
{
    if (!itsApp->init()) 
    {
        std::cerr << "Couldn't initialize application" << std::endl;
        return false;
    }
    itsApp->register_state_handler( std::bind(&VsomeipServer::onState, this, std::placeholders::_1));

    itsApp->register_subscription_handler(DMS_SERVICE_ID, DMS_INSTANCE_ID, DMS_EVENTGROUP_ID, std::bind(&VsomeipServer::onRegisterSubscriptionDMS, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4));
    
    std::set<vsomeip::eventgroup_t> DMS_groups;
    DMS_groups.insert(DMS_EVENTGROUP_ID);
    itsApp->offer_service(DMS_SERVICE_ID, DMS_INSTANCE_ID);
    itsApp->offer_event(DMS_SERVICE_ID, DMS_INSTANCE_ID, DMS_H264_IMG_EVENT_ID, DMS_groups);
    itsApp->offer_event(DMS_SERVICE_ID, DMS_INSTANCE_ID, DMS_DETECT_BBOX, DMS_groups);
    itsApp->offer_event(DMS_SERVICE_ID, DMS_INSTANCE_ID, DMS_KEYPOINT, DMS_groups);

    return true;
}

void VsomeipServer::start() 
{
    isRunning = true;
    notify_thread_ = std::thread(&VsomeipServer::run, this);
    itsVsomeipThread = std::thread([this](){
        itsApp->start();
    });
}

 void VsomeipServer::stop() 
{
    // Stop periodic notification thread first
    isRunning = false;
    if (notify_thread_.joinable()) 
    {
        notify_thread_.join();
    }
    
    itsApp->clear_all_handler();
    stop_offer();
    itsApp->stop();

    if(itsVsomeipThread.joinable())
    {
        itsVsomeipThread.join();
    }
    
}

void VsomeipServer::stop_offer() 
{
    itsApp->stop_offer_service(DMS_SERVICE_ID, DMS_INSTANCE_ID);
}

void VsomeipServer::onState(vsomeip::state_type_e _state) 
{
    std::cout << "Application " << itsApp->get_name() << " is " << (_state == vsomeip::state_type_e::ST_REGISTERED ? "registered." : "deregistered.") << std::endl;
}

bool VsomeipServer::onRegisterSubscriptionDMS(vsomeip::client_t clientId, vsomeip::uid_t, vsomeip::gid_t, bool subscribed)
{
    if(subscribed)
    {
        std::cout << "Client subscribed: 0x" << std::hex << clientId << std::dec << std::endl;
        std::cout << "DMS_SERVICE_ID: "    << std::hex << DMS_SERVICE_ID << std::dec << 
                     "DMS_INSTANCE_ID: "   << std::hex << DMS_INSTANCE_ID << std::dec << 
                     "DMS_EVENTGROUP_ID: " << std::hex << DMS_EVENTGROUP_ID << std::dec << std::endl;
    }
    return true;
}

// Periodic notification loop
void VsomeipServer::run() 
{

    while (isRunning) 
    {
        static uint32_t counter = 0;
        uint32_t current = counter++;

        auto payload2 = vsomeip::runtime::get()->create_payload();
        std::string DMS_Srv_string = "$GPDMS,123456.789,A,3954.6587,N,11623.3855,E,0.0,0.0,131025,0.0,E,A*75\r\n";
        std::vector<vsomeip::byte_t> DMS_payload_data(DMS_Srv_string.begin(), DMS_Srv_string.end());
        payload2->set_data(DMS_payload_data);
        
        itsApp->notify(DMS_SERVICE_ID, DMS_INSTANCE_ID, DMS_H264_IMG_EVENT_ID, payload2);
        itsApp->notify(DMS_SERVICE_ID, DMS_INSTANCE_ID, DMS_DETECT_BBOX, payload2);
        itsApp->notify(DMS_SERVICE_ID, DMS_INSTANCE_ID, DMS_KEYPOINT, payload2);

        std::this_thread::sleep_for(std::chrono::milliseconds(500));
    }
}

